#include <StackList.h>

// Set these to enable/disable components
#define USE_RFID 0
#define USE_LCD 0
#define USE_LED 1

#define SHOW_BARGRAPH USE_LED & 0
#define SHOW_AMBIENT USE_LED & 1

#define USE_DISPLAYDUINO 1

#include "Controller.h"
#include <RGB.h>
#include <RGBMatrix.h>
#include <MultiMatrix.h>
#include "StackArray.h"

#if USE_LCD == 1
#include <ST7565.h>
#include "LCD.h"
#endif

#if USE_RFID == 1 
#include <NewSoftSerial.h>
#include "RFID.h"
#endif

#if USE_LCD == 1

// Arduino - DisplayDuino Pins
// pin 9 - 19 - Serial data out (SID)
// pin 8 - 20 - Serial clock out (SCLK)
// pin 7 - 21 - Data/Command select (RS or A0)
// pin 6 - 22 - LCD reset (RST)
// pin 5 - 23 - LCD chip select (CS)
ST7565 glcd(19, 20, 21, 22, 23);

#endif

#if USE_RFID == 1

// connection to computer established (originally defined false in example)
boolean rfidConnected = true;
// software serial connection for rfid reader
NewSoftSerial rfidSerial =  NewSoftSerial(RFID_RX, RFID_TX);
// millis of last Reset 
unsigned int nowReset = 0;
// millis of last seen rfid tag
unsigned int nowLastRfid = 0;
// reader is in reset state
boolean rfidEnabled = false;
boolean rfidTagSeen = false;
// last seen tag
byte rfidTagCurrent[RFID_TAG_LENGTH];
// temp tag
byte rfidTagTemp[6];

#endif

StackArray<unsigned int> prevValues; 

// Initialize sonar, RFID, LCD, and LED Matrix
void setup() {

#if USE_DISPLAYDUINO == 1
  // Divide main clock by 2 to get 10 MHz
  CLKPR = 0b10000000;
  CLKPR = 0b00000001;
#endif
  
  // NOTE: The baud rate in the Arduino IDE serial monitor window or the Processing debug GUI must be half of what you set here, so 4800
  Serial.begin(19200);

#if USE_RFID == 1
  rfidSerial.begin(9600);

  // clear current tag
  clearTag(rfidTagCurrent, RFID_TAG_LENGTH);

  // RFID reset pins
  pinMode(RESET_PIN, OUTPUT);
  pinMode(RESET_LED_PIN, OUTPUT);
  Serial.println("RFID is enabled.");
#endif

#if USE_LCD == 1

  // initialize and set the contrast to 0x18
  glcd.begin(0x18);

#endif

  // Matrix LEDs
  MultiMatrix.begin(0, NORTH, 1, NORTH, 2, SOUTH, 3, SOUTH);
  MultiMatrix.clear();
  
  Serial.println("Hardware initialized...");
}

//ringNum 0 = outer ring; ringNum 8 = innermost ring
void fillRingNum(int ringNum, char color) {

  for ( int pixel = ringNum; pixel < 16 - ringNum; pixel++ ) {
    MultiMatrix.fillPixel(ringNum, pixel, color);
    MultiMatrix.fillPixel(pixel, ringNum, color);    
    MultiMatrix.fillPixel(16 - ringNum - 1, pixel, color);
    MultiMatrix.fillPixel(pixel, 16 - ringNum - 1, color);
  }

}

// TODO: can this be refactored a bit? a great deal of the code is repetitive.
// Probably, if there is time left for that.
void rings(char bgColor, char lineColor, boolean isGrowing, int rate, int iterations) {

  Serial.println("rings");

  if ( isGrowing ) {
    do {
      for ( int ringNum = 7; ringNum >= 0; ringNum-- ) {

        MultiMatrix.fillScreen(bgColor);
        if ( ringNum <= 4 ) {
          fillRingNum(ringNum + 3, lineColor);
        }
        if ( ringNum <= 1 ) {
          fillRingNum(ringNum + 6, lineColor);
        }

        fillRingNum(ringNum, lineColor);
        MultiMatrix.display();
        delay(rate);
      }

      for ( int ringNum = 2; ringNum >= 0; ringNum-- ) {
        MultiMatrix.fillScreen(bgColor);
        fillRingNum(ringNum, lineColor);
        fillRingNum(ringNum + 3, lineColor);
        MultiMatrix.display();
        delay(rate);
      }

      for ( int ringNum = 2; ringNum >= 0; ringNum-- ) {
        MultiMatrix.fillScreen(bgColor);
        fillRingNum(ringNum, lineColor);
        MultiMatrix.display();
        delay(rate);
      }
      MultiMatrix.fillScreen(bgColor);
      MultiMatrix.display();
    }

    while ( --iterations > 0 );    
  }
  //growing
  else {

    do {
      for ( int ringNum = 0; ringNum < 8; ringNum++ ) {

        MultiMatrix.fillScreen(bgColor);
        if ( ringNum >= 3 ) {
          fillRingNum(ringNum - 3, lineColor);
        }
        if ( ringNum >= 6 ) {
          fillRingNum(ringNum - 6, lineColor);
        }

        fillRingNum(ringNum, lineColor);
        MultiMatrix.display();
        delay(rate);
      }
      for ( int ringNum = 5; ringNum < 8; ringNum++ ) {
        MultiMatrix.fillScreen(bgColor);
        fillRingNum(ringNum - 3, lineColor);
        fillRingNum(ringNum, lineColor);
        MultiMatrix.display();
        delay(rate);
      }
      for ( int ringNum = 5; ringNum < 8; ringNum++ ) {
        MultiMatrix.fillScreen(bgColor);
        fillRingNum(ringNum, lineColor);
        MultiMatrix.display();
        delay(rate);
      }
      MultiMatrix.fillScreen(bgColor);
      MultiMatrix.display();
    }
    while ( --iterations > 0 );
  }
}

// I wanted to make this variable static in readMessage, but the compiler complained.
// Only readMessage should know about this var.
// Keep incoming characters in a linked list, to save difficulty with allocating char buffers
// This function will allocate memory for ppszOutMessage. The caller is responsible for freeing it.
StackList<char> stackCurrentMessage;
const unsigned int readMessage(char** ppszOutMessage) {
  
  // this only needs to happen once, but w/e.
  stackCurrentMessage.setPrinter(Serial);
  
  while (Serial.available() > 0) {
    stackCurrentMessage.push((char)Serial.read());

    if (stackCurrentMessage.peek() == MESSAGE_END) {
      const unsigned int lenMessage = stackCurrentMessage.count();
      *ppszOutMessage = (char*)malloc(sizeof(char) * lenMessage);
      
      for (int iCurrChar = lenMessage - 1; iCurrChar >= 0; iCurrChar--) {
        char currChar = stackCurrentMessage.pop();
        (*ppszOutMessage)[iCurrChar] = currChar;
      }
      
      return lenMessage;
    }
  }

  return 0;
}

const int readMessages() {
  char* pszMessage;
  unsigned int lenMessage = 0;
  while ((lenMessage = readMessage(&pszMessage)) > 0) {

    Serial.println(pszMessage);
    unsigned int currentEnergyLevel = atoi(pszMessage);
    free(pszMessage);

    if (prevValues.count() > MAX_SAVED_VALUES) {
      prevValues.removeLast(prevValues.count() - RETAIN_VALUES_COUNT);
    }

    prevValues.push(currentEnergyLevel);
  }

  return prevValues.isEmpty() ? -1 : prevValues.peek();
}

const unsigned int calcSlope() {
  if (prevValues.count() < AVG_COUNT * 2) {
    return NO_CHANGE;
  }

  StackArray<unsigned int> prevValuesCopy(prevValues);
  unsigned int recentSum = 0;
  for (int i = 0; i < AVG_COUNT; i++) {
    recentSum += prevValuesCopy.pop();
  }

  unsigned int olderSum = 0;
  for (int i = 0; i < AVG_COUNT; i++) {
    olderSum += prevValuesCopy.pop();
  }

  if (recentSum < olderSum - NO_CHANGE_THRESHOLD) {
    return DECREASING;
  } 
  else if (recentSum > olderSum + NO_CHANGE_THRESHOLD) {
    return INCREASING;
  }

  return NO_CHANGE;
}

int buffer[12] = {0, 100, 200, 300, 400, 450, 475, 500, 550, 600, 650, 700};
void loop() {
  
#if SHOW_BARGRAPH == 1
  barGraph(buffer, 12);
  MultiMatrix.display();
  delay(5000);
  return;
#endif

#if SHOW_AMBIENT == 1
  static unsigned int currentSlope = 0;
  static char colorToFill = BLUE;

  if (currentSlope == DECREASING || currentSlope == INCREASING) {
    rings(colorToFill, LINE_COLOR, currentSlope == INCREASING, RING_RATE, 1);
  }

  const int currentEnergyLevel = readMessages();
  if (currentEnergyLevel == -1) {
    return;
  }

  const unsigned int oldPrevValuesCount = prevValues.count();
  currentSlope = calcSlope(); 
  if (prevValues.count() != oldPrevValuesCount) {
    Serial.println("Error: calcSlope() had a side effect on prevValues!");
  }

  if (currentEnergyLevel < LOWER_GOAL_THRESHOLD) {
    colorToFill = UNDER_GOAL_COLOR;
  } 
  else if (currentEnergyLevel < UPPER_GOAL_THRESHOLD) {
    colorToFill = AT_GOAL_COLOR;
  } 
  else {
    colorToFill = OVER_GOAL_COLOR;
  }

  MultiMatrix.fillScreen(colorToFill);
  MultiMatrix.display();
#endif
}

void fillBar(int column, int value) {
  for ( int row = 14; row >= 0; row-- ) {
    if ( row >= 14-value ) {
      MultiMatrix.fillPixel(row, column, (row > 4 ? (row > 9 ? GREEN : ORANGE) : 
      RED));
    }
    else {
      MultiMatrix.fillPixel(row, column, BLACK);
    }
  }     
}

void barGraph ( int * buffer, int len ) {

  if (len > 12 || len < 1) { return; }
  
  MultiMatrix.clear(); 

  // draw the x and y axes
  MultiMatrix.fillRow(15, TEAL);
  MultiMatrix.fillColumn(15, TEAL);
  
  int start_column = 13;
  for ( int column = start_column; column > (start_column - len); column-- ) {
    int value = map(buffer[column], 0, UPPER_GOAL_THRESHOLD, 1, 14);
    Serial.print(buffer[column], DEC);
    Serial.print("-->");
    Serial.println(value, DEC);
    fillBar(column, value);
  }
}

// Clear RFID tags
#if USE_RFID == 1

// This is from the id12 example loop function
void checkID12() {

  byte action = 0;
  unsigned int now = millis();

  updateID12(false);

  // connection to computer established
  if(rfidConnected == true) {

    //    Serial.println("Connected...");
    clearTag(rfidTagTemp, 6);

    // serial connection with rfid reader
    if (rfidSerial.available()) {
      Serial.println("\nrfidSerial is available...");
      // wait for the next STX byte
      while(rfidSerial.available() && action != 0x02)
        action = rfidSerial.read();

      // STX byte found -> RFID tag available
      if(action == 0x02) {
        if(readID12(rfidTagTemp)) {
          nowLastRfid = millis();
          rfidTagSeen = true;
          updateCurrentRfidTag(rfidTagTemp);
        }
      }
    }
    else if(rfidEnabled && rfidTagSeen == true && (now - nowLastRfid) >= RFID_REMOVED_TIME) {    
      rfidTagSeen = false;
      updateCurrentRfidTag(rfidTagTemp);
    }
  }

  // serial connection with computer
  while (Serial.available() > 0) {
    // read action byte
    action = Serial.read();

    switch (action) 
    {
      // PING
    case 0x01:
      Serial.print(0x01, BYTE);

      // (C)ONNECT
    case 0x43:
      // clear serial
      while(rfidSerial.available()) rfidSerial.read();
      rfidConnected = true;
      break;

      // (D)ISCONNECT
    case 0x44:
      rfidConnected = false;
      break;
    }
  }

  // delay 100 milliseconds
  delay(100);

}

// TODO: Do we need this function in the long run?
// print actual RFID tag number to serial
void updateCurrentRfidTag(byte *tagNew)
{  
  // only print changed value     
  if(!equals(tagNew, rfidTagCurrent)) {

    saveTag(tagNew, rfidTagCurrent);
    byte i = 0;

    // STX
    Serial.print(0x02, BYTE);

    for (i=0; i<5; i++) {
      if (rfidTagCurrent[i] < 16) Serial.print("0");
      Serial.print(rfidTagCurrent[i], HEX);
    }

    // ETX
    Serial.print(0x03, BYTE);
  }
}

// read data from ID-12 reader
boolean readID12(byte *code)
{
  boolean result = false;
  byte val = 0;
  byte bytesIn = 0;
  byte tempbyte = 0;
  byte checksum = 0;

  // read 10 digit code + 2 digit checksum
  while (bytesIn < RFID_TAG_INPUT) {                        
    if( rfidSerial.available() > 0) { 
      val = rfidSerial.read();

      // if CR, LF, ETX or STX before the 10 digit reading -> stop reading
      if((val == 0x0D)||(val == 0x0A)||(val == 0x03)||(val == 0x02)) break;

      // Do Ascii/Hex conversion:
      if ((val >= '0') && (val <= '9')) 
        val = val - '0';
      else if ((val >= 'A') && (val <= 'F'))
        val = 10 + val - 'A';


      // Every two hex-digits, add byte to code:
      if (bytesIn & 1 == 1) {
        // make some space for this hex-digit by
        // shifting the previous hex-digit with 4 bits to the left:
        code[bytesIn >> 1] = (val | (tempbyte << 4));

        // If we're at the checksum byte, Calculate the checksum... (XOR)
        if (bytesIn >> 1 != RFID_TAG_LENGTH) checksum ^= code[bytesIn >> 1]; 
      } 
      else {
        // Store the first hex digit first...
        tempbyte = val;                           
      }

      // ready to read next digit
      bytesIn++;                                
    } 
  }

  // read complete
  if (bytesIn == RFID_TAG_INPUT) { 
    // valid tag
    if(code[5] == checksum) result = true; 
  }

  // reset id-12
  updateID12(true);

  return result;
}

// Update reset state of the RFID reader
void updateID12(boolean reset_)
{
  // reset is disabled
  if(RESET_ENABLED == 0 && rfidEnabled == true) return;

  // don't reset, just check if the id-12 should be enabled again 
  if(reset_ == false)
  {
    // current time
    unsigned int now = millis();

    // id-12 is disabled and ( reset period is over or initial id-12 startup )
    if (rfidEnabled == false && ((now - nowReset) >= RESET_TIME || nowReset == 0)) 
    { 
      digitalWrite(RESET_LED_PIN, LOW);
      digitalWrite(RESET_PIN, HIGH);
      rfidEnabled = true;
    }
  }
  // reset rfid reader
  else
  {
    digitalWrite(RESET_LED_PIN, HIGH);
    digitalWrite(RESET_PIN, LOW);

    nowReset = millis();
    rfidEnabled = false;  
  }

}

void clearTag(byte *arr, byte len)
{
  byte i;
  for (i=0; i < len ;i++) arr[i] = 0;
}

// Save RFID Tag
void saveTag(byte *tagIn, byte *tagOut)
{
  byte i;
  for (i=0; i < RFID_TAG_LENGTH ;i++) tagOut[i] = tagIn[i];
}

// Compre two RFID tags
boolean equals(byte *tag1, byte *tag2)
{
  boolean result = false;
  byte j;

  for (j=0; j < RFID_TAG_LENGTH ;j++) 
  {
    if(tag1[j] != tag2[j]) break;
    else if (j == RFID_TAG_LENGTH-1) result = true;
  }    
  return result;
}

#endif





